#ifndef _SCENE2RES_MATH_
#define _SCENE2RES_MATH_

#include <renderware.h>

#define _USE_MATH_DEFINES
#include <math.h>

#define clamp(a, lo, hi) (a > hi ? hi : (a < lo ? lo : a) )

inline double NormalizeRadians( double radians )
{
    radians = clamp( radians, -25.0f, 25.0f );

    while ( radians >= M_PI )
    {
        radians -= ( M_PI * 2 );
    }

    while ( radians < -M_PI )
    {
        radians += ( M_PI * 2 );
    }

    return radians;
}

#include "CQuat.h"

inline void QuatToEuler(
    double quatX, double quatY, double quatZ, double quatW,
    double& rotX, double& rotY, double& rotZ
)
{
    CQuat <double> myQuat( quatX, quatY, quatZ, quatW );

    rw::SquareMatrix <double, 3> matQuat;
    myQuat.ToMatrix <3> ( myQuat, matQuat );

    rw::VectorUtil::Normalize( matQuat[0] );
    rw::VectorUtil::Normalize( matQuat[1] );
    rw::VectorUtil::Normalize( matQuat[2] );

    matQuat.GetRotation( rotX, rotY, rotZ );
}

#endif //_SCENE2RES_MATH_